#region File information

/**
 * File: CollisionMath.cs
 * Date: 31-03-2008
 * Auth: Gerard v/d Lei
 * 
 * Desc: Contains collection of functions contained in the static class CollisionMath
 *       used for collision detection. Also contains a simple struct for a plane.
 */

#endregion

using Microsoft.Xna.Framework;
using System;
namespace Gremlin.Collision
{
    /// <summary>
    /// Simple structure for plane
    /// </summary>
    public struct TPlane
    {
        public TPlane(Vector3 normal, float d)
        {
            _normal = normal;
            _d = d;
        }
        public Vector3 _normal;
        public float _d;
    }

    /// <summary>
    /// Class with static methods with some specialized math for collisions.
    /// Some functions are not present in XNA or were not optimal and are
    /// rewritten.
    /// Note: function are kept as primitive as possible, less overhead, more re-usability
    /// </summary>
    class CollisionMath
    {
        /// <summary>
        /// Checks if a ray intersects the bounding sphere.
        /// </summary>
        /// <param name="origin">Origin of the ray</param>
        /// <param name="delta">Direction of the ray</param>
        /// <returns>True when an intersection occurs</returns>
        public static bool RaySphereIntersection(Vector3 rayOrigin, Vector3 rayDelta, Vector3 sphereCenter, float sphereRadius)
        {
            Vector3 diff = sphereCenter - rayOrigin;
            double c1 = Vector3.Dot(diff, rayDelta);
            if (c1 <= 0)
                return diff.LengthSquared() < (sphereRadius * sphereRadius);

            double c2 = Vector3.Dot(rayDelta, rayDelta);
            if (c2 <= c1)
                return ((diff + rayDelta).LengthSquared()) < (sphereRadius * sphereRadius);

            double b = c1 / c2;
            Vector3 Pb = rayOrigin + ((float)b) * rayDelta;
            return ((Pb - sphereCenter).LengthSquared()) < (sphereRadius * sphereRadius);
        }

        /// <summary>
        /// Calculates where a ray intersects with the plane on which this side lies.
        /// </summary>
        /// <param name="origin">Origin of the ray</param>
        /// <param name="delta">Direction of the ray</param>
        /// <param name="point">Point where ray intersects</param>
        /// <returns>True when intersection is found, false otherwise</returns>
        public static bool RayPlaneIntersection(Vector3 rayOrigin, Vector3 rayDelta, Vector3 planeNormal, Vector3 pointInPlane, out Vector3 point)
        {
            point = new Vector3(0, 0, 0);
            float b = Vector3.Dot(planeNormal, rayDelta);
            if (System.Math.Abs(b) < 0.0005f) return false;
            float a = -Vector3.Dot(planeNormal, rayOrigin - pointInPlane);
            float r = a / b;
            if (r < 0) return false;
            if (r > 1) return false;
            point.X = rayOrigin.X + (rayDelta.X * r);
            point.Y = rayOrigin.Y + (rayDelta.Y * r);
            point.Z = rayOrigin.Z + (rayDelta.Z * r);

            return true;
        }

        /// <summary>
        /// Checks if a point is inside a triangle.
        /// </summary>
        /// <param name="A">Point of triangle between point B and C</param>
        /// <param name="B">Point of triangle next to point A</param>
        /// <param name="C">Point of triangle next to point A</param>
        /// <param name="p">Point to check if it is in triangle(C,A,B)</param>
        /// <returns>
        /// 1 when p lies in the triangle(C,A,B)
        /// 0 when p lies between the sides C-A and B-A with infinite length</returns>
        /// -1 otherwise
        public static int PointInTriangle(Vector3 A, Vector3 B, Vector3 C, Vector3 p)
        {
            Vector3 v0 = C - A;
            Vector3 v1 = B - A;
            Vector3 v2 = p - A;

            float dot00 = Vector3.Dot(v0, v0);
            float dot01 = Vector3.Dot(v0, v1);
            float dot02 = Vector3.Dot(v0, v2);
            float dot11 = Vector3.Dot(v1, v1);
            float dot12 = Vector3.Dot(v1, v2);

            float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
            float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
            float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

            if (u < 0 || v < 0)
                return -1; //Point lies outside triangle and every convex which has the sides C-A and B-A
            if ((u > 0) && (v > 0) && (u + v < 1))
                return 1; //Point lies in Triangle
            else
                return 0; //Point does not lie in triangle, but could be in convex made of sides C-A and B-A
        }

        /// <summary>
        /// Convert a point to BaryCentric coordinates
        /// </summary>
        /// <param name="e0">First edge, edge runs from (0,0,0) to e0</param>
        /// <param name="e1">Second edge, edge runs from (0,0,0) to e1</param>
        /// <param name="p">Point to convert</param>
        /// <returns>The Bary Centric Coordinates</returns>
        public static Vector2 ToBaryCentricCoordinates(Vector3 e0, Vector3 e1, Vector3 p)
        {
            float dot00 = Vector3.Dot(e0, e0);
            float dot01 = Vector3.Dot(e0, e1);
            float dot02 = Vector3.Dot(e0, p);
            float dot11 = Vector3.Dot(e1, e1);
            float dot12 = Vector3.Dot(e1, p);

            float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
            float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
            float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
            return new Vector2(u, v);
        }

        /// <summary>
        /// Return signed distance from point to plane
        /// </summary>
        /// <param name="planeNormal">Normal of plane</param>
        /// <param name="pointOnPlane">A point on plane</param>
        /// <param name="p">Point to check distance from</param>
        /// <returns>Signed distance to plane, positive when behind plane</returns>
        public static float DistancePointToPlane(Vector3 planeNormal, Vector3 pointOnPlane, Vector3 p)
        {
            float angle = -Vector3.Dot(planeNormal, p - pointOnPlane);
            return angle;
        }

        /// <summary>
        /// Return signed distance from point to plane
        /// </summary>
        /// <param name="planeNormal">Normal of plane</param>
        /// <param name="pointOnPlane">A point on plane</param>
        /// <param name="p">Point to check distance from</param>
        /// <returns>Signed distance to plane, positive when behind plane</returns>
        public static float DistancePointToPlane(Vector3 planeNormal, float planeD, Vector3 p)
        {
            float angle = Vector3.Dot(planeNormal, p) + planeD;
            return angle;
        }
    }
}
